'''
K210侧盘点代码

K210需要记得第一问中每一步应当怎样走（似乎没有时间做第二问了，到时候再说吧）

todo list：
1.command_table里的每一步，移动类型不用改，但移动长度/角度一定要实际跑一跑、微调一下
2.现在盘点和无线数据传输已经分别实现了，把它们整合到一起
'''
import image, sensor, lcd, time
from maix import GPIO
from machine import Timer
from machine import UART #串口库函数
from fpioa_manager import fm # GPIO重定向函数


# 摄像头配置
sensor.reset() # 初始化.
sensor.set_framesize(sensor.QVGA) # 使用QVGA 320*240或QQVGA 160*120.
sensor.set_vflip(0) # 关闭摄像头后置模式
sensor.set_hmirror(0) # 关闭水平翻转
sensor.set_pixformat(sensor.GRAYSCALE) # 使用灰度图像，追踪路线
sensor.set_auto_gain(True)
sensor.set_auto_whitebal(True) # 灰度图像需要使用白平衡和颜色追踪
sensor.skip_frames(time = 2000) # 等待摄像头稳定.

'''通过以下表格告知小车每一步应当怎样走，格式与sending_data一样。实际测试时需要将小车尾部与出发点左边界相切。再定义标志位，在STM32执行完命令之前不必再发送移动命
令，此时其值为False（但可以发送二维码信息，并且一旦检测到二维码应当立即下达新的移动命令，因为它们作为参照物是要对当前坐标进行校准的。但直到二维码消失都不必再发送，
应当再定义标志位）。串口只能传递整数，所以在这个过程中必然会出现精度丢失的问题。'''
command_sending_flag = True
code_sending_flag = True # 当还没发送二维码时其值为True，此时可以正常发送二维码信息，并使得i++，下一次会传输下一个命令，否则不传输
command_table = [
    [0, 34],
    [2, 78],

    [0, 10],

    [0, 30],
    [0, 30],
    [0, 65], # 第一组盘点完毕
    [1, 80],
    [0, 8],
    [1, 80],
    [0, 5],
    [0, 30],
    [0, 30], # 第二组盘点完毕

    [0, 60],
    [2, 80],
    [0, 40],
    [2, 78],

    [0, 5],
    [0, 30],
    [0, 30],
    [0, 70], # 第三组盘点完毕
    [1, 79],
    [0, 8],
    [1, 74],
    [0, 5],
    [0, 30],
    [0, 30], # 第四组盘点完毕

    [2, 80],
    [0, 20],
    [2, 78],
    [0, 62], # 以上为第一问的固定盘点部分

    [2, 0],
    [0, 0],
    [2, 0],
    [0, 0],
    [2, 0],
    [0, 0],
    [2, 0],
    [0, 0]  # 最后面八个列表属于第二问，移动类型是确定的，但移动距离需要根据抽取到的二维码进行实时演算，此处赋的0仅仅是在形式上进行占位，演算相应的程序中需要修改参数
]
total_steps = len(command_table) - 8

#LCD初始化
lcd.init()

fm.register(18, fm.fpioa.UART1_TX, force=True)
fm.register(19, fm.fpioa.UART1_RX, force=True) # 设置18、19引脚分别为串口输出、输入引脚

fm.register(3, fm.fpioa.UART2_TX, force=True)
fm.register(2, fm.fpioa.UART2_RX, force=True)

uart_A = UART(UART.UART1, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)
uart_B = UART(UART.UART2, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)

# 按说在python标准语法里数字字符可以隐式地转化成对应的整数，不知道为什么这里不行，因此使用了字典进行显式转换
str_to_int = {'1':1, '2':2, '3':3, 'A':10, 'B':11, 'C':12, 'D':13}

def sending_data(x, y, command_type):
    if command_type == "move":
        time.sleep_ms(500)
        FH = bytes([0xFF, x, y, 0, 0xFE])  # 下达关于小车要如何运动的命令。对于x:0表示前进，1表示右转，2表示左转，y表示距离或角度
        uart_A.write(FH) # 串口发送数据，其中的十六进制数字是帧头、帧尾
    else:
        FH = str_to_int[x[4][0]] * 16 + str_to_int[x[4][1]]   # 传递货物信息，其余的，像位置坐标等是可以忽略的。货物信息涉及的字母
        # 刚好在十六进制表示的范围内，于是直接将其转化为整数进行存储，再以十六进制拼回来，再加以显示。
        while True:
            uart_B.write(bytes([FH])) # 串口发送数据
            time.sleep_ms(500)
            tmp = uart_B.read()
            if tmp != None:
                print(tmp)
                flag = False
                break

i = 0

question = 1

while True:
    if i < total_steps and question == 1:
        img = sensor.snapshot() # 实时拍照.
        #if code_sending_flag:
        for code in img.find_qrcodes():
            #img.draw_rectangle(code.rect(), color = 127, thickness=3)
            sending_data(code, 0, "code")  # 查询二维码，有的话就发送出去、传新的命令，然后继续待机。一定时间内不用再次发送数据、下达新命令
            #code_sending_flag = False
                #tim.start()
        '''
        不知为何，上面查询到二维码、使得命令发送标志位为True之后，下面的语句虽然会执行，但串口不会发消息，可能是因为二维码信息还在发送吧。于是多加了一个机制：只要
        STM32不回复收到就会一直发。
        现在看来是波特率设置地不匹配，但还是保留着这个功能。
        '''
        if command_sending_flag:
            try:
                move_type, value = command_table[i]  # 移动类型与距离（角度）
                i += 1
            except:
                lcd.draw_string(100, 100, "inventory finished", lcd.RED, lcd.BLACK)
                lcd.draw_string(30, 120, "please start running question two", lcd.RED, lcd.BLACK)
            sending_data(move_type, value, "move")
            read_data = uart_A.read()
            if read_data != None:
                print(read_data)
            command_sending_flag = False
        else:
            read_data = uart_A.read()
            if read_data != None:
                print(read_data)
            if read_data == b'\xff\x03\xfe':  # STM32只需要传这一种命令，表示已完成了对于命令的执行
                command_sending_flag = True
        lcd.display(img)
    elif question != 2:
        lcd.draw_string(100, 100, "inventory finished", lcd.RED, lcd.BLACK)# 共三十步，跑完第一题就结束了，等待跑第二题
        lcd.draw_string(30, 120, "please start running question two", lcd.RED, lcd.BLACK)
        command_sending_flag = False
    else:
        if i != total_steps:# 因为第二问没有自己独立的一张命令表，所以在遍历的时候一定要确保它是从应有位置开始的。当然此处纯粹是调试时才会触发，正常写题时
            i = total_steps  # 一定是问题一跑完了的，不会需要这个语句
        img = sensor.snapshot() # 实时拍照.
        for code in img.find_qrcodes():
            img.draw_rectangle(code.rect(), color = 127, thickness=3)
            sending_data(code, 0, "code")  # 查询二维码，有的话就发送出去、传新的命令，然后继续待机。一定时间内不用再次发送数据、下达新命令
            code_sending_flag = False
            # 根据二维码的内容计算接下来要怎么走。此处的值需要修改
            # 第一类：确定其在哪一列
            if code[4][0] == 'D':
                command_table[total_steps + 1][1] = 35
                command_table[total_steps + 5][1] = 35
            elif code[4][0] == 'C':
                command_table[total_steps + 1][1] = 58
                command_table[total_steps + 5][1] = 58
            elif code[4][0] == 'B':
                command_table[total_steps + 1][1] = 119
                command_table[total_steps + 5][1] = 119
            else:
                command_table[total_steps + 1][1] = 142
                command_table[total_steps + 5][1] = 142
            # 第二类：确定其在哪一行
            if code[4][1] == '3':
                command_table[total_steps + 3][1] = 10
                command_table[total_steps + 7][1] = 10
            elif code[4][1] == '2':
                command_table[total_steps + 3][1] = 40
                command_table[total_steps + 7][1] = 40
            else:
                command_table[total_steps + 3][1] = 70
                command_table[total_steps + 7][1] = 70
            # 第三类：根据具体点位说不定会不同的旋转角
            command_table[total_steps][1] = 80
            command_table[total_steps + 2][1] = 80
            command_table[total_steps + 4][1] = 80
            command_table[total_steps + 6][1] = 80

            code_sending_flag = False

            total_steps += 8
            question = 1
        lcd.display(img)
